#include "gtd_common.hpp"
#include "gtd_cloud.hpp"
#include "gtd_profiler.hpp"

NAMESPACE_BEG(gtd)

#define SPEED 0.2

//Screen attributes
int SCREEN_WIDTH = 640;
int SCREEN_HEIGHT = 480;
const int SCREEN_BPP = 32;

//The frame rate
const int FRAMES_PER_SECOND = 60;

//The attributes of the square
const int SQUARE_WIDTH = 20;
const int SQUARE_HEIGHT = 20;

//Event handler
SDL_Event event;

//Global variables
struct Point {	
	float x,y,z;
	Point (float a, float b, float c):x(a),y(b),z(c){}	
};

vector< Point > points;
GLfloat projMatrix[16];
//----------------


//The square
class Square
{
    private:
    //The offsets
    int x, y;

    //The velocity of the square
    int xVel, yVel;

    public:
    //Initializes
    Square();

    //Handles key presses
    void handle_input();

    //Moves the square
    void move();

    //Shows the square on the screen
    void show();
};

//The timer
class Timer
{
    private:
    //The clock time when the timer started
    int startTicks;

    //The ticks stored when the timer was paused
    int pausedTicks;

    //The timer status
    bool paused;
    bool started;

    public:
    //Initializes variables
    Timer();

    //The various clock actions
    void start();
    void stop();
    void pause();
    void unpause();

    //Gets the timer's time
    int get_ticks();

    //Checks the status of the timer
    bool is_started();
    bool is_paused();
};

bool init_GL()
{
    //Set clear color
    glClearColor( 0, 0, 0, 0 );

    //Set projection
    glMatrixMode( GL_PROJECTION );
    glLoadIdentity();
    //glOrtho( 0, SCREEN_WIDTH, SCREEN_HEIGHT, 0, -1, 1 );
	//gluPerspective( 45, SCREEN_WIDTH/SCREEN_HEIGHT, 0.1, 100 );
	gluPerspective( 45, 1, 1, 1000);
//	glGetFloatv(GL_PROJECTION_MATRIX, pMatrix);


    //Initialize modelview matrix
    glMatrixMode( GL_MODELVIEW );
    glLoadIdentity();

    //If there was any errors
    if( glGetError() != GL_NO_ERROR )
    {
        return false;
    }

    //If everything initialized
    return true;
}

bool init()
{
    //Initialize SDL
    if( SDL_Init( SDL_INIT_EVERYTHING ) < 0 )
    {
        return false;
    }

    //Create Window
    if( SDL_SetVideoMode( SCREEN_WIDTH, SCREEN_HEIGHT, SCREEN_BPP, SDL_OPENGL ) == NULL )
    {
        return false;
    }

    //Initialize OpenGL
    if( init_GL() == false )
    {
        return false;
    }

    //Set caption
    SDL_WM_SetCaption( "OpenGL Test", NULL );

    return true;
}

void clean_up()
{
    //Quit SDL
    SDL_Quit();
}

Square::Square()
{
    //Initialize offsets
    x = 0;
    y = 0;

    //Initialize velocity
    xVel = 0;
    yVel = 0;
}

void Square::handle_input()
{
    //If a key was pressed
    if( event.type == SDL_KEYDOWN )
    {
        //Adjust the velocity
        switch( event.key.keysym.sym )
        {
			case SDLK_UP: yVel -= (int) (SPEED * SQUARE_HEIGHT / 2); break;
            case SDLK_DOWN: yVel += (int) (SPEED * SQUARE_HEIGHT / 2); break;
            case SDLK_LEFT: xVel -= int (SPEED * SQUARE_WIDTH / 2); break;
            case SDLK_RIGHT: xVel += (int) (SPEED * SQUARE_WIDTH / 2); break;
        }
    }
    //If a key was released
    else if( event.type == SDL_KEYUP )
    {
        //Adjust the velocity
        switch( event.key.keysym.sym )
        {
            case SDLK_UP: yVel += (int) (SPEED * SQUARE_HEIGHT) / 2; break;
            case SDLK_DOWN: yVel -= (int) (SPEED * SQUARE_HEIGHT) / 2; break;
            case SDLK_LEFT: xVel += (int) (SPEED * SQUARE_WIDTH) / 2; break;
            case SDLK_RIGHT: xVel -= (int) (SPEED * SQUARE_WIDTH) / 2; break;
        }
    }
}

void Square::move()
{
    //Move the square left or right
    x += xVel;

    //If the square went too far
    if( ( x < 0 ) || ( x + SQUARE_WIDTH > SCREEN_WIDTH ) )
    {
        //Move back
        //x -= xVel;
    }

    //Move the square up or down
    y += yVel;

    //If the square went too far
    if( ( y < 0 ) || ( y + SQUARE_HEIGHT > SCREEN_HEIGHT ) )
    {
        //Move back
        //y -= yVel;
    }
}

void Square::show()
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);		// Clear The Screen And The Depth Buffer
	glLoadIdentity();
	
	glTranslatef(0.0f,0.0f,-6.0f);	
	glRotatef(y,1,0,0);
	glRotatef(x,0,1,0);
	glPointSize(5);
	glBegin(GL_POINTS);	
	
	for (unsigned int i=0; i<points.size(); ++i) {
		glVertex3f(points[i].x, points[i].y, points[i].z);
	}
	glEnd();

    //Reset
   // glLoadIdentity();
}

Timer::Timer()
{
    //Initialize the variables
    startTicks = 0;
    pausedTicks = 0;
    paused = false;
    started = false;
}

void Timer::start()
{
    //Start the timer
    started = true;

    //Unpause the timer
    paused = false;

    //Get the current clock time
    startTicks = SDL_GetTicks();
}

void Timer::stop()
{
    //Stop the timer
    started = false;

    //Unpause the timer
    paused = false;
}

void Timer::pause()
{
    //If the timer is running and isn't already paused
    if( ( started == true ) && ( paused == false ) )
    {
        //Pause the timer
        paused = true;

        //Calculate the paused ticks
        pausedTicks = SDL_GetTicks() - startTicks;
    }
}

void Timer::unpause()
{
    //If the timer is paused
    if( paused == true )
    {
        //Unpause the timer
        paused = false;

        //Reset the starting ticks
        startTicks = SDL_GetTicks() - pausedTicks;

        //Reset the paused ticks
        pausedTicks = 0;
    }
}

int Timer::get_ticks()
{
    //If the timer is running
    if( started == true )
    {
        //If the timer is paused
        if( paused == true )
        {
            //Return the number of ticks when the timer was paused
            return pausedTicks;
        }
        else
        {
            //Return the current time minus the start time
            return SDL_GetTicks() - startTicks;
        }
    }

    //If the timer isn't running
    return 0;
}

bool Timer::is_started()
{
    return started;
}

bool Timer::is_paused()
{
    return paused;
}

void sfm();


ifstream feat1_in("img1features.txt");
ifstream feat2_in("img2features.txt");

int main( int argc, char *argv[] )
{
	feat1_in >> SCREEN_WIDTH >> SCREEN_HEIGHT;
	feat1_in.ignore();
	feat1_in.ignore();

    //Quit flag
    bool quit = false;

    //Initialize
    if( init() == false )
    {
        return 1;
    }

    //Our square object
    Square square; 

    //The frame rate regulator
    Timer fps;

	//Recover 3D Points
	sfm();

	//Wait for user exit
	while( quit == false )
	{
        //Start the frame timer
        fps.start();

        //While there are events to handle
		while( SDL_PollEvent( &event ) )
		{
            //Handle key presses
            square.handle_input();

			if( event.type == SDL_QUIT )
			{
                quit = true;
            }
		}


	    //Move the square
	    square.move();

	    //Clear the screen
	    glClear( GL_COLOR_BUFFER_BIT );

	    //Show the square
	    square.show();

	    //Update screen
	    SDL_GL_SwapBuffers();

        //Cap the frame rate
        if( fps.get_ticks() < 1000 / FRAMES_PER_SECOND )
        {
            SDL_Delay( ( 1000 / FRAMES_PER_SECOND ) - fps.get_ticks() );
        }
	}

	//Clean up
	clean_up();

	return 0;
}


//----SFM----


struct CorMat {
	static const int COLS = 9;
	int rows;
	vector< vector<float> > e;	
};

void readInput(vector<CorMat> &cormats) {
	
	ifstream fin("pairs.bin", ios::in|ios_base::binary);
	int rowNum;
	float buf;
	CorMat mat;

	//while (true) {
	for (int cm=0; cm<50; ++cm) {
		fin.read((char *)&rowNum,4);
		/*if (fin.eof()) {
			break;
		}*/
		mat.e.resize(rowNum);
		mat.rows = rowNum;
		//cout << rowNum << endl;
		for (int row=0; row<rowNum; ++row) {
			//read in a correspondece matrix Mx9
			mat.e[row].resize(mat.COLS);
			for (int col=0; col<mat.COLS; ++col) {
				fin.read((char *)&buf,4);
				mat.e[row][col] = buf;
			}
		}
		//add it to the cormats vector
		cormats.push_back(mat);
	}
	return;
}




struct Point2i {
	int x;
	int y;
};

void readInputCal(vector<CorMat> &cormats) {
	
	vector< Point2i > img1, img2;

	int rowNum;
	float buf;
	CorMat mat;

	Point2i p;
	while(feat1_in >> p.x >> p.y) {
		feat1_in.ignore();
		img1.push_back(p);	
	}


	//skip the dimensions
	feat2_in >> p.x >> p.y;
	feat2_in.ignore();
	feat2_in.ignore();

	while(feat2_in >> p.x >> p.y) {
		feat2_in.ignore();
		img2.push_back(p);
	}

	if (img1.size() != img2.size()) {
		cout << "Mismatch of features.." << endl;
	}
	mat.e.resize(img1.size());
	mat.rows = img1.size();

	int N = img1.size();

	Point2i n,o;

	for (int i=0; i<N; ++i) {
		o = img1[i];
		n = img2[i];
		mat.e[i].resize(mat.COLS);
		mat.e[i][0] = n.x*o.x;
		mat.e[i][1] = n.x*o.y;
		mat.e[i][2] = n.x;
		mat.e[i][3] = n.y*o.x;
		mat.e[i][4] = n.y*o.y;
		mat.e[i][5] = n.y;
		mat.e[i][6] = o.x;
		mat.e[i][7] = o.y;
		mat.e[i][8] = 1;
	}

	cormats.push_back(mat);
	return;
}


void fill(TNT::Array2D<float> &M, const int val) {
	int nrows=M.dim1(), ncols=M.dim2();
	for (int r=0; r<nrows; ++r) {
		for (int c=0; c<ncols; ++c) {
			M[r][c] = val;
		}
	}
}

void fill(TNT::Array1D<float> &V, const int val) {
	for (int i=0; i<V.dim(); ++i) {
		V[i] = val;
	}
}


TNT::Array2D<float> get3DPoint(int x, int y, int x2, int y2, TNT::Array2D<float> P, TNT::Array2D<float> P2) {
	TNT::Array2D<float> A(4,4);
	TNT::Array2D<float> s, U, V;

	for (int i=0; i<4; ++i) {
		A[0][i] = x * P[2][i] - P[0][i];
		A[1][i] = y * P[2][i] - P[1][i];
		A[2][i] = x2 * P2[2][i] - P2[0][i];
		A[3][i] = y2 * P2[2][i] - P2[1][i];
	}

	//normalize	
	float len;
	for (int row=0; row<4; ++row) {
		len = 0.;
		for (int col=0; col<4; ++col) {
			len += (A[row][col] * A[row][col]);
		}
		len = sqrt(len);
		for (int col=0; col<4; ++col) {
			A[row][col] /= len;
		}
	}
	

	JAMA::SVD<float> svd(A);
	svd.getV(V);

	/*for (int i=0; i<4; ++i) {
		cout << V[0][i] << ' ';
	}
	cout << endl;*/
	TNT::Array2D<float> p(3,1);
	p[0][0] = V[0][0]/V[0][3];
	p[1][0] = V[0][1]/V[0][3];
	p[2][0] = V[0][2]/V[0][3];

	return p;
}	

void combine(TNT::Array2D<float> R, TNT::Array2D<float> T, TNT::Array2D<float> & RT) {
	vector<float> dp;

	for (int i=0; i<3; ++i) {
		for (int j=0; j<3; ++j) {
			RT[i][j] = R[i][j];
		}
	}
	for (int i=0; i<3; ++i) {
		RT[i][3] = T[i][0];
	}
	
}


void transpose(TNT::Array2D<float> &V) {
	float tmp;
	for (int i=0; i<3; ++i) {
		for (int j=i; j<3; ++j) {
			tmp = V[i][j];
			V[i][j] = V[j][i];
			V[j][i] = tmp;
		}
	}
}

void sfm() {
	//vector of matrices Y (correspondences)
	vector<CorMat> cormats;
	readInputCal(cormats);
	//The input is not normalised, but we'll try run the 8-point anyway.

	//Try to run it on a single correnspondence matrix first
	//We already have matrix A in Ax=0, so now just run least-squares -> SVD

	TNT::Array2D<float> W(3,3), Wt(3,3), T1(3,1), T2(3,1), R1(3,3), R2(3,3);
	TNT::Array2D<float> allR(3,3), allT(3,1);
	fill(allR,0);
	fill(allT,0);


	for (uint cm=0; cm<cormats.size(); ++cm) {
		int r = cormats[cm].rows;
		int c = cormats[cm].COLS;
		
		TNT::Array2D<float> s, U, V;
		TNT::Array2D<float> Y(r,c);		

		for (int i=0; i<r; ++i) {
			for (int j=0; j<c; ++j) {
				//cout << i << ' ' << j << endl;
				Y[i][j] = cormats[cm].e[i][j];
			}
		}

		JAMA::SVD<float> svd(Y);
	    
		svd.getV(V);
		
		TNT::Array2D<float> F(3,3), K(3,3), E(3,3);
		fill(K,0);

		for (int i=0; i<3; ++i) {
			for (int j=0; j<3; ++j) {
				F[i][j] = V[i*3+j][8];
			}
		}

		cout << "F:\n" << F << endl;

		//get essential matrix=-
		K[0][0] = 689.4;
		K[1][1] = 689.4;
		K[2][2] = 1;
		K[0][2] = 301.7;
		K[1][2] = 244.3;
		cout << "K:\n" << K << endl;

		E = TNT::matmult(F,K);
		transpose(K);
		E = TNT::matmult(K,E);

		//Apply singularity constraint
		svd = JAMA::SVD<float>(E);
		
		svd.getS(s);
		svd.getU(U);
		svd.getV(V);

		//set the last main diagonal entry of s to 0
		s[2][2] = 0;
				
		//transpose V
		transpose(V);
		//reverse svd
//		cout << E << endl;
		E = TNT::matmult(s,V);
		E = TNT::matmult(U,E);
		cout << "E:\n" << E << endl;


		E[0][0] = 0.0214;
		E[0][1] = 0.2858;
		E[0][2] = -0.1269;
		E[1][0] = -0.0743;
		E[1][1] = 0.0654;
		E[1][2] = 0.9885;
		E[2][0] = -0.0406;
		E[2][1] = -0.9552;
		E[2][2] = 0.0264;

		//Now we have E. Get R and T.
		//Find 4 pairs of possible R and T. A' means A-Transpose
		//E = USV', R = UWV' or UW'V', T = u3 or -u3. (ui is the i-th column of U)
		//We have already computed V' (it is in V)


		fill(W,0);
		fill(Wt,0);

		//Setup W and Wt
		Wt[1][0] = W[0][1] = -1;
		Wt[0][1] = W[1][0] = 1;
		Wt[2][2] = W[2][2] = 1;

		//Get T1 and T2
		for (int i=0; i<3; ++i) {
			T1[i][0] = U[i][2];
			T2[i][0] = -U[i][2];
		}

		//Get R1 and R2
		R1 = TNT::matmult(W,V);
		R1 = TNT::matmult(U,R1);
		R2 = TNT::matmult(Wt,V);
		R2 = TNT::matmult(U,R2);

		
		//Now we have all the candidates for R and T.		

		TNT::Array2D<float> Proj(3,4);		
		fill(Proj, 0);
		Proj[0][0] = 1;
		Proj[1][1] = 1;
		Proj[2][2] = 1;
		
		TNT::Array2D<float> RT(3,4);
		TNT::Array2D<float> out(3,1);
		
		cout << "R1:\n" << R1 << endl;
		combine(R1,T1,RT);
		//combine(R1,T2,RT);
		//combine(R2,T1,RT);
		//combine(R2,T2,RT);
		cout << "P:\n" << Proj << endl;
		cout << "P'\n:" << RT << endl;
		
		for (int p=0; p<r; ++p) {
			out = get3DPoint(Y[p][6],Y[p][7],Y[p][2],Y[p][5], Proj, RT);

			//

			//float ox = Y[p][6], oy = Y[p][7],
			//		nx = Y[p][2], ny = Y[p][5];

			////Method from wiki:
			//TNT::Array2D<float> top(1,3),yp(3,1);
			//fill(top,0);
			//fill(yp,0);
			//for (int i=0; i<3; ++i) {
			//	top[0][i] = R1[1][i] - ny*R1[2][i];					
			//}

			//yp[0][0] = ox;
			//yp[1][0] = oy;
			//yp[2][0] = 1;

			//float a = matmult(top,T1)[0][0];
			//float b = matmult(top,yp)[0][0];
			//out[2][0] = a/b;
			//out[0][0] = out[2][0] * ox;
			//out[1][0] = out[2][0] * oy;



			//cout << endl;
			//cout << out << endl << "---" << endl;;
			//out = allR * out;
			/*TNT::Array2D<float> tout(3,1);
			for (int i=0; i<3; ++i) {
				tout[i][0] = 0;
				for (int j=0; j<3; ++j) {
					tout[i][0] += allR[i][j]*out[j][0];
				}
			}*/

			//cout << out << endl << endl;
			//out = out + allT;
			
			points.push_back(Point(out[0][0],out[1][0],out[2][0]));
			//cout << p << endl;
		
			//combine(R1,T2,RT);
			//get3DPoint(Y[p][6],Y[p][7],Y[p][2],Y[p][5], P, RT);

			//combine(R2,T1,RT);
			//get3DPoint(Y[p][6],Y[p][7],Y[p][2],Y[p][5], P, RT);

			//combine(R2,T2,RT);
			//get3DPoint(Y[p][6],Y[p][7],Y[p][2],Y[p][5], P, RT);
		}
		//allR = matmult(R2,allR);
		//allT = matmult(allT,T1);
	}

	return;
	
	//run sfm on every cormat, add 3d points to the 'points' array.
}

PointCloud::PointCloud(int width, int height)
{

}

PointCloud::~PointCloud()
{

}

void PointCloud::update(const std::vector<FeaturePair>& pairs)
{
    PROFILE("Point Cloud Construction");

}

NAMESPACE_END(gtd)
